%Creates particles that are guaranteed to be within a map from a list of
%states
function particles = create_particles_from_states(state, weights, map)
    sdd = .05; %TODO: These two values need tuning.
    sdt = .1;
    newx = state(:, 1) + sdd*randn(size(state, 1), 1);
    newy = state(:, 2) + sdd*randn(size(state, 1), 1);
    newth = state(:, 3) + sdt*randn(size(state, 1), 1);
    newstate = [newx, newy, newth];
    particles = [newstate, weights];
    %Check to see if the newstate is in the map, and if not, repeat
    %generation. Should this be the case?
    %if inpolygon(newx, newy, map(:, 1), map(:, 2)) ~= 1
    %    particle = create_particle_from_state(state, map);
    %end
    %inpoly = ~inpolygon(particles(:,1), particles(:, 2), map(:, 1), map(:,2));
    particles = particles(inpolygon(particles(:,1), particles(:, 2), map(:, 1), map(:,2)), :);
    %while any(inpoly)
    %    particles(inpoly, 1:2) = [state(inpoly, 1) + sdd*randn(size(state(inpoly), 1), 1), state(inpoly, 2) + sdd*randn(size(state(inpoly), 1), 1)];
    %    inpoly = ~inpolygon(particles(:,1), particles(:, 2), map(:, 1), map(:,2));
    %nd
    return
end